#ifndef __Matrix3_hpp__
#define __Matrix3_hpp__

#include "Vector3.hpp"

namespace ge {

class Matrix4;
class Quaternion;

class Matrix3
{
 public:

     Matrix3( void );

     Matrix3( const Matrix3& _m );

     Matrix3( const Matrix4& _m );

     Matrix3( const Quaternion& _q );

     Matrix3( const float _v[9] );

     Matrix3( float _m00, float _m01, float _m02,
              float _m10, float _m11, float _m12, 
              float _m20, float _m21, float _m22 );

     float* operator [] ( uint _row );

     const float* operator [] ( uint _row ) const;

     float& operator () ( uint _row, uint _col );

     const float& operator () ( uint _row, uint _col ) const;

     float* operator * ( void );

     const float* operator * ( void ) const;

     Matrix3& operator = ( const Matrix3& _m );

     Matrix3& operator = ( const Matrix4& _m );

     Matrix3& operator = ( const Quaternion& _q );

     Vector3 Row( uint _row ) const;

     Matrix3& SetRow( uint _row, float _x, float _y, float _z );

     Matrix3& SetRow( uint _row, const float _v[3] );

     Matrix3& SetRow( uint _row, const Vector3& _v );

     Vector3 Col( uint _col ) const;

     Matrix3& SetCol( uint _col, float _x, float _y, float _z );

     Matrix3& SetCol( uint _col, const float _v[3] );

     Matrix3& SetCol( uint _col, const Vector3& _v );

     Matrix3& Set( float _m00, float _m01, float _m02,
                   float _m10, float _m11, float _m12, 
                   float _m20, float _m21, float _m22 );

     Matrix3& Set( const Matrix3& _m );

     Matrix3& Set( const Matrix4& _m );

     Matrix3& Set( const Quaternion& _q );

     Matrix3& FromPtr( const float _v[9] );

     Matrix3& SetZero( void );

     Matrix3& SetIdentity( void );

     bool operator == ( const Matrix3& _m ) const;

     bool operator != ( const Matrix3& _m ) const;

     bool HasScale( void ) const;

     Matrix3 operator - ( void ) const;

     const Matrix3& operator + ( void ) const;

     Matrix3 operator + ( const Matrix3& _m ) const;

     Matrix3 operator - ( const Matrix3& _m ) const;

     Matrix3 operator * ( const Matrix3& _m ) const;

     Matrix3& operator *= ( const Matrix3& _m );

     Vector3 operator * ( const Vector3& _v ) const; 

     friend Vector3 operator * ( const Vector3& _v, const Matrix3& _m );

     Matrix3 operator * ( float _s ) const;

     friend Matrix3 operator * ( float _s, const Matrix3& _m );

     Matrix3& FromAxes( const Vector3& _x, const Vector3& _y, const Vector3& _z );

     void ToAxes( Vector3& _x, Vector3& _y, Vector3& _z ) const;

     Matrix3& FromAxisAngle( const Vector3& _axis, float _angle );

     void ToAxisAngle( Vector3& _axis, float& _angle ) const;

     Matrix3& FromEulerAngles( const Vector3& _euler );
     
     Matrix3& FromEulerAngles( float _yaw, float _pitch, float _roll );

     bool ToEulerAngles( Vector3& _euler ) const;

     bool ToEulerAngles( float& _yaw, float& _pitch, float& _roll ) const;

     void Decomposition( Matrix3& _q, Vector3& _d, Vector3& _u ) const;

     float Determinant( void ) const;

     bool HasInverse( float _tolerance = Math::Epsilon ) const;

     Matrix3& Inverse( float _tolerance = Math::Epsilon );

     Matrix3 InverseCopy( float _tolerance = Math::Epsilon ) const;

     Matrix3& Orthonormalize( void );

     Matrix3 OrthonormalizedCopy( void ) const;

     Matrix3& Transpose( void );

     Matrix3 TransposedCopy( void ) const;

 public:

     static const Matrix3 Zero;
     static const Matrix3 Identity;

 public:

     union
     {
          struct 
          { 
               float m00, m01, m02; 
               float m10, m11, m12; 
               float m20, m21, m22; 
          };

          float m[3][3]; // [row][col]
          float v[9];
     };
};

//----------------------------------------------------------------------------//
// Matrix3
//----------------------------------------------------------------------------//

//----------------------------------------------------------------------------//
inline Matrix3::Matrix3( void )
{
}
//----------------------------------------------------------------------------//
inline Matrix3::Matrix3( const Matrix3& _m )
{
     memcpy( v, _m.v, sizeof( float ) * 9 );
}
//----------------------------------------------------------------------------//
inline Matrix3::Matrix3( const float _v[9] )
{
     memcpy( v, _v, sizeof( float ) * 9 );
}
//----------------------------------------------------------------------------//
inline Matrix3::Matrix3( float _m00, float _m01, float _m02,
                  float _m10, float _m11, float _m12, 
                  float _m20, float _m21, float _m22 ) : m00( _m00 ), m01( _m01 ), m02( _m02 ),
                                                         m10( _m10 ), m11( _m11 ), m12( _m12 ),
                                                         m20( _m20 ), m21( _m21 ), m22( _m22 )
{
}
//----------------------------------------------------------------------------//
inline float* Matrix3::operator [] ( uint _row )
{
     return (float*)(m + _row);
}
//----------------------------------------------------------------------------//
inline const float* Matrix3::operator [] ( uint _row ) const
{
     return (const float*)(m + _row);
}
//----------------------------------------------------------------------------//
inline float& Matrix3::operator () ( uint _row, uint _col )
{
     return m[_row][_col]; 
}
//----------------------------------------------------------------------------//
inline const float& Matrix3::operator () ( uint _row, uint _col ) const
{
     return m[_row][_col]; 
}
//----------------------------------------------------------------------------//
inline float* Matrix3::operator * ( void )
{
     return v;
}
//----------------------------------------------------------------------------//
inline const float* Matrix3::operator * ( void ) const
{
     return v;
}
//----------------------------------------------------------------------------//
inline Matrix3& Matrix3::operator = ( const Matrix3& _m )
{
     memcpy( v, _m.v, sizeof( float ) * 9 );
     return *this;
}
//----------------------------------------------------------------------------//
inline Matrix3& Matrix3::operator = ( const Matrix4& _m )
{
     return Set( _m );
}
//----------------------------------------------------------------------------//
inline Matrix3& Matrix3::operator = ( const Quaternion& _q )
{
     return Set( _q );
}
//----------------------------------------------------------------------------//
inline Vector3 Matrix3::Row( uint _row ) const
{
     return Vector3( m[_row] );
}
//----------------------------------------------------------------------------//
inline Matrix3& Matrix3::SetRow( uint _row, float _x, float _y, float _z )
{
     m[_row][0] = _x;
     m[_row][1] = _y;
     m[_row][2] = _z;
     return *this;
}
//----------------------------------------------------------------------------//
inline Matrix3& Matrix3::SetRow( uint _row, const float _v[3] )
{
     m[_row][0] = _v[0];
     m[_row][1] = _v[1];
     m[_row][2] = _v[2];
     return *this;
}
//----------------------------------------------------------------------------//
inline Matrix3& Matrix3::SetRow( uint _row, const Vector3& _v )
{
     m[_row][0] = _v.x;
     m[_row][1] = _v.y;
     m[_row][2] = _v.z;
     return *this;
}
//----------------------------------------------------------------------------//
inline Vector3 Matrix3::Col( uint _col ) const
{
     return Vector3( m[0][_col], m[1][_col], m[2][_col] );
}
//----------------------------------------------------------------------------//
inline Matrix3& Matrix3::SetCol( uint _col, float _x, float _y, float _z )
{
     m[0][_col] = _x;
     m[1][_col] = _y;
     m[2][_col] = _z;
     return *this;
}
//----------------------------------------------------------------------------//
inline Matrix3& Matrix3::SetCol( uint _col, const float _v[3] )
{
     m[0][_col] = _v[0];
     m[1][_col] = _v[1];
     m[2][_col] = _v[2];
     return *this;
}
//----------------------------------------------------------------------------//
inline Matrix3& Matrix3::SetCol( uint _col, const Vector3& _v )
{
     m[0][_col] = _v.x;
     m[1][_col] = _v.y;
     m[2][_col] = _v.z;
     return *this;
}
//----------------------------------------------------------------------------//
inline Matrix3& Matrix3::Set( float _m00, float _m01, float _m02,
                       float _m10, float _m11, float _m12, 
                       float _m20, float _m21, float _m22 )
{ 
     m00 = _m00; m01 = _m01; m02 = _m02;
     m10 = _m10; m11 = _m11; m12 = _m12;
     m20 = _m20; m21 = _m21; m22 = _m22;
     return *this;
}
//----------------------------------------------------------------------------//
inline Matrix3& Matrix3::Set( const Matrix3& _m )
{
     return (*this = _m);
}
//----------------------------------------------------------------------------//
inline Matrix3& Matrix3::FromPtr( const float _v[9] )
{
     memcpy( v, _v, sizeof( float ) * 9 );
     return *this;
}
//----------------------------------------------------------------------------//
inline Matrix3& Matrix3::SetZero( void )
{
     v[0] = 0.0f; v[1] = 0.0f; v[2] = 0.0f;
     v[3] = 0.0f; v[4] = 0.0f; v[5] = 0.0f;
     v[6] = 0.0f; v[7] = 0.0f; v[8] = 0.0f;
     return *this;
}
//----------------------------------------------------------------------------//
inline Matrix3& Matrix3::SetIdentity( void )
{
     m00 = 1.0f; m01 = 0.0f; m02 = 0.0f;
     m10 = 0.0f; m11 = 1.0f; m12 = 0.0f;
     m20 = 0.0f; m21 = 0.0f; m22 = 1.0f;
     return *this;
}
//----------------------------------------------------------------------------//
inline bool Matrix3::operator == ( const Matrix3& _m ) const
{
     return v[0] == _m.v[0] &&
            v[1] == _m.v[1] &&
            v[2] == _m.v[2] &&
            v[3] == _m.v[3] &&
            v[4] == _m.v[4] &&
            v[5] == _m.v[5] &&
            v[6] == _m.v[6] &&
            v[7] == _m.v[7] &&
            v[8] == _m.v[8];
}
//----------------------------------------------------------------------------//
inline bool Matrix3::operator != ( const Matrix3& _m ) const
{
     return !(*this == _m);
}
//----------------------------------------------------------------------------//
inline bool Matrix3::HasScale( void ) const
{
     if( !Math::Equals( m00 * m00 + m10 * m10 + m20 * m20, 1.0f, Math::Epsilon ) ) return true;
     if( !Math::Equals( m01 * m01 + m11 * m11 + m21 * m21, 1.0f, Math::Epsilon ) ) return true;
     if( !Math::Equals( m02 * m02 + m12 * m12 + m22 * m22, 1.0f, Math::Epsilon ) ) return true;
     return false;
}
//----------------------------------------------------------------------------//
inline Matrix3 Matrix3::operator - ( void ) const
{
     return Matrix3( -m00, -m01, -m02, -m10, -m11, -m12, -m20, -m21, -m22 );
}
//----------------------------------------------------------------------------//
inline const Matrix3& Matrix3::operator + ( void ) const
{
     return *this;
}
//----------------------------------------------------------------------------//
inline Matrix3 Matrix3::operator + ( const Matrix3& _m ) const
{
     return Matrix3( m00 + _m.m00, m01 + _m.m01, m02 + _m.m02,
                     m10 + _m.m10, m11 + _m.m11, m12 + _m.m12,
                     m20 + _m.m20, m21 + _m.m21, m22 + _m.m22 );
}
//----------------------------------------------------------------------------//
inline Matrix3 Matrix3::operator - ( const Matrix3& _m ) const
{
     return Matrix3( m00 - _m.m00, m01 - _m.m01, m02 - _m.m02,
                     m10 - _m.m10, m11 - _m.m11, m12 - _m.m12,
                     m20 - _m.m20, m21 - _m.m21, m22 - _m.m22 );
}
//----------------------------------------------------------------------------//
inline Matrix3 Matrix3::operator * ( const Matrix3& _m ) const
{
     return Matrix3( m00 * _m.m00 + m01 * _m.m10 + m02 * _m.m20,
                     m00 * _m.m01 + m01 * _m.m11 + m02 * _m.m21,
                     m00 * _m.m02 + m01 * _m.m12 + m02 * _m.m22,
                     m10 * _m.m00 + m11 * _m.m10 + m12 * _m.m20,
                     m10 * _m.m01 + m11 * _m.m11 + m12 * _m.m21,
                     m10 * _m.m02 + m11 * _m.m12 + m12 * _m.m22,
                     m20 * _m.m00 + m21 * _m.m10 + m22 * _m.m20,
                     m20 * _m.m01 + m21 * _m.m11 + m22 * _m.m21,
                     m20 * _m.m02 + m21 * _m.m12 + m22 * _m.m22 );
}
//----------------------------------------------------------------------------//
inline Matrix3& Matrix3::operator *= ( const Matrix3& _m )
{
     return (*this = ( *this * _m ));
}
//----------------------------------------------------------------------------//
inline Vector3 Matrix3::operator * ( const Vector3& _v ) const
{
     return Vector3( m00 * _v.x + m01 * _v.y + m02 * _v.z,
                     m10 * _v.x + m11 * _v.y + m12 * _v.z,
                     m20 * _v.x + m21 * _v.y + m22 * _v.z );
} 
//----------------------------------------------------------------------------//
inline Vector3 operator * ( const Vector3& _v, const Matrix3& _m )
{
     return _m * _v;
}
//----------------------------------------------------------------------------//
inline Matrix3 Matrix3::operator * ( float _s ) const
{
     return Matrix3( m00 * _s, m01 * _s, m02 * _s,
                     m10 * _s, m11 * _s, m12 * _s,
                     m20 * _s, m21 * _s, m22 * _s );
}
//----------------------------------------------------------------------------//
inline Matrix3 operator * ( float _s, const Matrix3& _m )
{
     return Matrix3( _s * _m.m00, _s * _m.m01, _s * _m.m02,
                     _s * _m.m10, _s * _m.m11, _s * _m.m12,
                     _s * _m.m20, _s * _m.m21, _s * _m.m22 );
}
//----------------------------------------------------------------------------//
inline Matrix3& Matrix3::FromAxes( const Vector3& _x, const Vector3& _y, const Vector3& _z )
{
     SetCol( 0, _x );
     SetCol( 1, _y );
     SetCol( 2, _z );
     return *this;
}
//----------------------------------------------------------------------------//
inline void Matrix3::ToAxes( Vector3& _x, Vector3& _y, Vector3& _z ) const
{
     _x = Col( 0 );
     _y = Col( 1 );
     _z = Col( 2 );
}
//----------------------------------------------------------------------------//
inline Matrix3& Matrix3::FromAxisAngle( const Vector3& _axis, float _angle )
{
     float _s, _c, _omc, _x2, _y2, _z2, _xym, _xzm, _yzm, _xs, _ys, _zs;

     Math::SinCos( _angle, _s, _c );

     _omc = 1.0f - _c;
     _x2  = _axis.x * _axis.x;
     _y2  = _axis.y * _axis.y;
     _z2  = _axis.z * _axis.z;
     _xym = _axis.x * _axis.y * _omc;
     _xzm = _axis.x * _axis.z * _omc;
     _yzm = _axis.y * _axis.z * _omc;
     _xs  = _axis.x * _s;
     _ys  = _axis.y * _s;
     _zs  = _axis.z * _s;

     m00  = _x2 * _omc + _c;
     m01  = _xym - _zs;
     m02  = _xzm + _ys;
     m10  = _xym + _zs;
     m11  = _y2 * _omc + _c;
     m12  = _yzm - _xs;
     m20  = _xzm - _ys;
     m21  = _yzm + _xs;
     m22  = _z2 * _omc + _c;

     return *this;
}
//----------------------------------------------------------------------------//
inline void Matrix3::ToAxisAngle( Vector3& _axis, float& _angle ) const
{
     _angle = Math::ACos( ( m00 + m11 + m22 - 1.0f ) * 0.5f );

     if( _angle <= 0.0f )
     {
          _axis.Set( 1.0f, 0.0f, 0.0f );
     }
     else
     {
          if( _angle < Math::Pi )
          {
               _axis.Set( m21 - m12, m02 - m20, m10 - m01 ).Normalize();
          }
          else
          {
               float _h_inv;
                    
               if( m00 >= m11 )
               {
                    if( m00 >= m22 )
                    {
                         _axis.x = Math::RSqrt( m00 - m11 - m22 + 1.0f ) * 0.5f;
                         _h_inv = 0.5f / _axis.x;
                         _axis.y = _h_inv * m01;
                         _axis.z = _h_inv * m02;
                    }
                    else
                    {
                         _axis.z = Math::RSqrt( m22 - m00 - m11 + 1.0f ) * 0.5f;
                         _h_inv = 0.5f / _axis.z;
                         _axis.x = _h_inv * m02;
                         _axis.y = _h_inv * m12;
                    }
               }
               else
               {
                    if( m11 >= m22 )
                    {
                         _axis.y = Math::RSqrt( m11 - m00 - m22 + 1.0f ) * 0.5f;
                         _h_inv  = 0.5f / _axis.y;
                         _axis.x = _h_inv * m01;
                         _axis.z = _h_inv * m12;
                    }
                    else
                    {
                         _axis.z = Math::RSqrt( m22 - m00 - m11 + 1.0f ) * 0.5f;
                         _h_inv = 0.5f / _axis.z;
                         _axis.x = _h_inv * m02;
                         _axis.y = _h_inv * m12;
                    }
               }
          }
     }
}
//----------------------------------------------------------------------------//
inline Matrix3& Matrix3::FromEulerAngles( const Vector3& _euler )
{
     return FromEulerAngles( _euler.x, _euler.y, _euler.z );
}
//----------------------------------------------------------------------------//
inline Matrix3& Matrix3::FromEulerAngles( float _yaw, float _pitch, float _roll ) // XYZ
{ 
#     if 1

     float _cy,_sy,_cp,_sp,_cr,_sr,_spcr,_spsr;

     Math::SinCos( _yaw, _sy, _cy );
     Math::SinCos( _pitch, _sp, _cp );
     Math::SinCos( _roll, _sr, _cr );
     _spcr = _sp*_cr;
     _spsr = _sp*-_sr;

     return Set( _cp*_cr,                _cp*-_sr,               _sp,
                 _cy*_sr + -_sy*-_spcr,  _cy*_cr + -_sy*-_spsr,  -_sy*_cp,
                 _sy*_sr + _cy*-_spcr,   _sy*_cr + _cy*-_spsr,   _cy*_cp );

#     else

     float _c, _s;

     Math::SinCos( _yaw, _s, _c );
     Matrix3 _xm( 1.0f, 0.0f, 0.0f, 0.0f, _c, -_s, 0.0f, _s, _c );

     Math::SinCos( _pitch, _s, _c );
     Matrix3 _ym( _c, 0.0f, _s, 0.0f, 1.0f, 0.0f, -_s, 0.0f, _c );

     Math::SinCos( _roll, _s, _c );
     Matrix3 _zm( _c, -_s, 0.0f, _s, _c, 0.0f, 0.0f, 0.0f, 1.0f );

     return (*this = _xm * ( _ym * _zm ) );

#     endif
}
//----------------------------------------------------------------------------//
inline bool Matrix3::ToEulerAngles( Vector3& _euler ) const
{
     return ToEulerAngles( _euler.x, _euler.y, _euler.z );
}
//----------------------------------------------------------------------------//
inline bool Matrix3::ToEulerAngles( float& _yaw, float& _pitch, float& _roll ) const // XYZ
{ 
#     if 1 

     if( Math::Abs( m02 ) >= 1 )
     {
          float _delta = Math::ATan2( m00, m20 );
          _yaw = 0;
          _roll = Math::HalfPi + _delta;
          _pitch = ( ( m02 > 0 ) ? Math::HalfPi : -Math::HalfPi );
     }
     else
     {
          _pitch = Math::ASin( m02 );
          float _ipc = 1.0f / Math::Cos( _pitch );
          _yaw = Math::ATan2( -m12 * _ipc, m22 * _ipc );
          _roll = Math::ATan2( -m01 * _ipc, m00 * _ipc );
     }

     return true;

#     else

     _pitch = Math::ASin( m02 );

     if( _pitch < Math::HalfPi )
     {
          if( _pitch > -Math::HalfPi )
          {
               _yaw = Math::ATan2( -m12, m22 );
               _roll = Math::ATan2( -m01, m00 );
               return true;
          }
          else
          {
              _pitch = Math::Clamp( _pitch, -Math::HalfPi, Math::HalfPi );
              _roll = 0.0f;
              _yaw = _roll - Math::ATan2( m10, m11 );
          }
     }
     else
     {
         _pitch = Math::Clamp( _pitch, -Math::HalfPi, Math::HalfPi );
         _roll = 0.0f;
         _yaw = Math::ATan2( m10, m11 ) - _roll;
     }
         
     return false;

#     endif
}
//----------------------------------------------------------------------------//
inline void Matrix3::Decomposition( Matrix3& _q, Vector3& _d, Vector3& _u ) const
{
     float _invl = Math::RSqrt( m00 * m00 + m10 * m10 + m20 * m20 );
     _q.m00 = m00 * _invl;
     _q.m10 = m10 * _invl;
     _q.m20 = m20 * _invl;

     float _dot = ( _q.m00 * m01 + _q.m10 * m11 + _q.m20 * m21 );
     _q.m01 = m01 - ( _dot * _q.m00 );
     _q.m11 = m11 - ( _dot * _q.m10 );
     _q.m21 = m21 - ( _dot * _q.m20 );

     _invl = Math::RSqrt( _q.m01 * _q.m01 + _q.m11 * _q.m11 + _q.m21 * _q.m21 );
     _q.m01 *= _invl;
     _q.m11 *= _invl;
     _q.m21 *= _invl;

     _dot = ( _q.m00 * m02 + _q.m10 * m12 + _q.m20 * m22 );
     _q.m02 = m02 - ( _dot * _q.m00 );
     _q.m12 = m12 - ( _dot * _q.m10 );
     _q.m22 = m22 - ( _dot * _q.m20 );

     _dot = ( _q.m01 * m02 + _q.m11 * m12 + _q.m21 * m22 );
     _q.m02 -= _dot * _q.m01;
     _q.m12 -= _dot * _q.m11;
     _q.m22 -= _dot * _q.m21;

     _invl = Math::RSqrt( _q.m02 * _q.m02 + _q.m12 * _q.m12 + _q.m22 * _q.m22 );
     _q.m02 *= _invl;
     _q.m12 *= _invl;
     _q.m22 *= _invl;

     float _det = ( _q.m00 * _q.m11 * _q.m22 + _q.m01 * _q.m12 * _q.m20 +
                    _q.m02 * _q.m10 * _q.m21 - _q.m02 * _q.m11 * _q.m20 -
                    _q.m01 * _q.m10 * _q.m22 - _q.m00 * _q.m12 * _q.m21 );

     if( _det < 0.0f )
     {
          _q.v[0] = - _q.v[0];
          _q.v[1] = - _q.v[1];
          _q.v[2] = - _q.v[2];
          _q.v[3] = - _q.v[3];
          _q.v[4] = - _q.v[4];
          _q.v[5] = - _q.v[5];
          _q.v[6] = - _q.v[6];
          _q.v[7] = - _q.v[7];
          _q.v[8] = - _q.v[8];
     }

     Matrix3 _r;
     _r.m00 = ( _q.m00 * m00 + _q.m10 * m10 + _q.m20 * m20 );
     _r.m01 = ( _q.m00 * m01 + _q.m10 * m11 + _q.m20 * m21 );
     _r.m11 = ( _q.m01 * m01 + _q.m11 * m11 + _q.m21 * m21 );
     _r.m02 = ( _q.m00 * m02 + _q.m10 * m12 + _q.m20 * m22 );
     _r.m12 = ( _q.m01 * m02 + _q.m11 * m12 + _q.m21 * m22 );
     _r.m22 = ( _q.m02 * m02 + _q.m12 * m12 + _q.m22 * m22 );

     _d.x = _r.m00;
     _d.y = _r.m11;
     _d.z = _r.m22;

     float _invd0 = 1.0f / _d.x;
     _u.x = _r.m01 * _invd0;
     _u.y = _r.m02 * _invd0;
     _u.z = _r.m12 / _d.y;
}
//----------------------------------------------------------------------------//
inline float Matrix3::Determinant( void ) const
{
     return ( m00 * ( m11 * m22 - m12 * m21 ) + m01 * ( m12 * m20 - m10 * m22 ) + m02 * ( m10 * m21 - m11 * m20 ) );
}
//----------------------------------------------------------------------------//
inline bool Matrix3::HasInverse( float _tolerance ) const
{
     return Math::Abs( Determinant() ) > _tolerance;
}
//----------------------------------------------------------------------------//
inline Matrix3& Matrix3::Inverse( float _tolerance )
{
     Matrix3 _tmp;

     _tmp.m00 = ( m11 * m22 - m12 * m21 );
     _tmp.m01 = ( m02 * m21 - m01 * m22 );
     _tmp.m02 = ( m01 * m12 - m02 * m11 );
     _tmp.m10 = ( m12 * m20 - m10 * m22 );
     _tmp.m11 = ( m00 * m22 - m02 * m20 );
     _tmp.m12 = ( m02 * m10 - m00 * m12 );
     _tmp.m20 = ( m10 * m21 - m11 * m20 );
     _tmp.m21 = ( m01 * m20 - m00 * m21 );
     _tmp.m22 = ( m00 * m11 - m01 * m10 );

     float _d = ( m00 * _tmp.m00 + m01 * _tmp.m10 + m02 * _tmp.m20 );

     if( Math::Abs( _d ) > _tolerance )
     {
          float _invd = 1.0f / _d;

          Set( _tmp.m00 * _invd, _tmp.m01 * _invd, _tmp.m02 * _invd,
               _tmp.m10 * _invd, _tmp.m11 * _invd, _tmp.m12 * _invd,
               _tmp.m20 * _invd, _tmp.m21 * _invd, _tmp.m22 * _invd );
     }
     // else { assert( false && "Could not make inverse matrix" ); }

     return *this;
}
//----------------------------------------------------------------------------//
inline Matrix3 Matrix3::InverseCopy( float _tolerance ) const
{
     return Matrix3( *this ).Inverse( _tolerance );
}
//----------------------------------------------------------------------------//
inline Matrix3& Matrix3::Orthonormalize( void )
{
     float _invl = Math::RSqrt( m00 * m00 + m10 * m10 + m20 * m20 );

     m00 *= _invl;
     m10 *= _invl;
     m20 *= _invl;

     float _dot0 = ( m00 * m01 + m10 * m11 + m20 * m21 );

     m01 -= ( _dot0 * m00 );
     m11 -= ( _dot0 * m10 );
     m21 -= ( _dot0 * m20 );

     _invl = Math::RSqrt( m01 * m01 + m11 * m11 + m21 * m21 );

     m01 *= _invl;
     m11 *= _invl;
     m21 *= _invl;

     float _dot1 = ( m01 * m02 + m11 * m12 + m21 * m22 );

     _dot0 = ( m00 * m02 + m10 * m12 + m20 * m22 );

     m02 -= ( _dot0 * m00 + _dot1 * m01 );
     m12 -= ( _dot0 * m10 + _dot1 * m11 );
     m22 -= ( _dot0 * m20 + _dot1 * m21 );

     _invl = Math::RSqrt( m02 * m02 + m12 * m12 + m22 * m22 );

     m02 *= _invl;
     m12 *= _invl;
     m22 *= _invl;

     return *this;
}
//----------------------------------------------------------------------------//
inline Matrix3 Matrix3::OrthonormalizedCopy( void ) const
{
     return Matrix3( *this ).Orthonormalize();
}
//----------------------------------------------------------------------------//
inline Matrix3& Matrix3::Transpose( void )
{
     // 00 01 02    00 10 20
     // 10 11 12 -> 01 11 21
     // 20 21 22    02 12 22
     
     Swap( m01, m10 );
     Swap( m02, m20 );
     Swap( m12, m21 );
     return *this;
}
//----------------------------------------------------------------------------//
inline Matrix3 Matrix3::TransposedCopy( void ) const
{
     return Matrix3( *this ).Transpose();
}
//----------------------------------------------------------------------------//


} // namespace ge

#endif//__Matrix3_hpp__
